/*
 * armboot - Startup Code for OMAP3530/ARM Cortex CPU-core
 *
 * Copyright (c) 2004	Texas Instruments <r-woodruff2@ti.com>
 *
 * Copyright (c) 2001	Marius Gröger <mag@sysgo.de>
 * Copyright (c) 2002	Alex Züpke <azu@sysgo.de>
 * Copyright (c) 2002	Gary Jennejohn <garyj@denx.de>
 * Copyright (c) 2003	Richard Woodruff <r-woodruff2@ti.com>
 * Copyright (c) 2003	Kshitij <kshitij@ti.com>
 * Copyright (c) 2006-2008 Syed Mohammed Khasim <x0khasim@ti.com>
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include <asm-offsets.h>
#include <config.h>
#include <version.h>
#include <asm/system.h>
#include <linux/linkage.h>

/*************************************************************************
 *
 * Startup Code (reset vector)
 *
 * do important init only if we don't start from memory!
 * setup Memory and board specific bits prior to relocation.
 * relocate armboot to ram
 * setup stack
 *
 *************************************************************************/
.globl	_TEXT_BASE
_TEXT_BASE:
	.word	CONFIG_SYS_TEXT_BASE

	.globl	reset

reset:
	bl	save_boot_params
	/*
	 * disable interrupts (FIQ and IRQ), also set the cpu to SVC32 mode,
	 * except if in HYP mode already
	 */
	mrs	r0, cpsr
	and	r1, r0, #0x1f		@ mask mode bits
	teq	r1, #0x1a		@ test for HYP mode
	bicne	r0, r0, #0x1f		@ clear all mode bits
	orrne	r0, r0, #0x13		@ set SVC mode
	orr	r0, r0, #0xc0		@ disable FIQ and IRQ
	msr	cpsr,r0

/*
 * Setup vector:
 * (OMAP4 spl TEXT_BASE is not 32 byte aligned.
 * Continue to use ROM code vector only in OMAP4 spl)
 */
#if !(defined(CONFIG_OMAP44XX) && defined(CONFIG_SPL_BUILD))
	/* Set V=0 in CP15 SCTRL register - for VBAR to point to vector */
	mrc	p15, 0, r0, c1, c0, 0	@ Read CP15 SCTRL Register
	bic	r0, #CR_V		@ V = 0
	mcr	p15, 0, r0, c1, c0, 0	@ Write CP15 SCTRL Register

	/* Set vector address in CP15 VBAR register */
	ldr	r0, =_start
	mcr	p15, 0, r0, c12, c0, 0	@Set VBAR
#endif

	/* the mask ROM code should have PLL and others stable */
#ifndef CONFIG_SKIP_LOWLEVEL_INIT
	bl	cpu_init_cp15
	bl	cpu_init_crit
#endif

	bl	_main

/*------------------------------------------------------------------------------*/

ENTRY(c_runtime_cpu_setup)
/*
 * If I-cache is enabled invalidate it
 */
#ifndef CONFIG_SYS_ICACHE_OFF
	mcr	p15, 0, r0, c7, c5, 0	@ invalidate icache
	mcr     p15, 0, r0, c7, c10, 4	@ DSB
	mcr     p15, 0, r0, c7, c5, 4	@ ISB
#endif
/*
 * Move vector table
 */
	/* Set vector address in CP15 VBAR register */
	ldr     r0, =_start
	mcr     p15, 0, r0, c12, c0, 0  @Set VBAR

	bx	lr

ENDPROC(c_runtime_cpu_setup)

/*************************************************************************
 *
 * void save_boot_params(u32 r0, u32 r1, u32 r2, u32 r3)
 *	__attribute__((weak));
 *
 * Stack pointer is not yet initialized at this moment
 * Don't save anything to stack even if compiled with -O0
 *
 *************************************************************************/
ENTRY(save_boot_params)
	bx	lr			@ back to my caller
ENDPROC(save_boot_params)
	.weak	save_boot_params

/*************************************************************************
 *
 * cpu_init_cp15
 *
 * Setup CP15 registers (cache, MMU, TLBs). The I-cache is turned on unless
 * CONFIG_SYS_ICACHE_OFF is defined.
 *
 *************************************************************************/
ENTRY(cpu_init_cp15)
	/*
	 * Invalidate L1 I/D
	 */
	mov	r0, #0			@ set up for MCR
	mcr	p15, 0, r0, c8, c7, 0	@ invalidate TLBs
	mcr	p15, 0, r0, c7, c5, 0	@ invalidate icache
	mcr	p15, 0, r0, c7, c5, 6	@ invalidate BP array
	mcr     p15, 0, r0, c7, c10, 4	@ DSB
	mcr     p15, 0, r0, c7, c5, 4	@ ISB

	/*
	 * disable MMU stuff and caches
	 */
	mrc	p15, 0, r0, c1, c0, 0
	bic	r0, r0, #0x00002000	@ clear bits 13 (--V-)
	bic	r0, r0, #0x00000007	@ clear bits 2:0 (-CAM)
	orr	r0, r0, #0x00000002	@ set bit 1 (--A-) Align
	orr	r0, r0, #0x00000800	@ set bit 11 (Z---) BTB
#ifdef CONFIG_SYS_ICACHE_OFF
	bic	r0, r0, #0x00001000	@ clear bit 12 (I) I-cache
#else
	orr	r0, r0, #0x00001000	@ set bit 12 (I) I-cache
#endif
	mcr	p15, 0, r0, c1, c0, 0

#ifdef CONFIG_ARM_A53
	/*
	 *clear AFE,TRE bit in sctrl
	 *non-secure: reset value is unknow
	 *secure    : default value is 0
	 *notice    : we must set the TRE bit to enable the memory
	 *            arttribute configuration from the section table.
	 */
	MRC     p15, 0, r0, c1, c0, 0   @Read SCTLR
	BIC     r0, r0, #(1<<28)        @clr TRE bit
	BIC     r0, r0, #(1<<29)        @clr AEF bit
	MCR     p15, 0, r0, c1, c0, 0   @Write SCTLR
#endif

#ifdef CONFIG_ARM_ERRATA_716044
	mrc	p15, 0, r0, c1, c0, 0	@ read system control register
	orr	r0, r0, #1 << 11	@ set bit #11
	mcr	p15, 0, r0, c1, c0, 0	@ write system control register
#endif

#if (defined(CONFIG_ARM_ERRATA_742230) || defined(CONFIG_ARM_ERRATA_794072))
	mrc	p15, 0, r0, c15, c0, 1	@ read diagnostic register
	orr	r0, r0, #1 << 4		@ set bit #4
	mcr	p15, 0, r0, c15, c0, 1	@ write diagnostic register
#endif

#ifdef CONFIG_ARM_ERRATA_743622
	mrc	p15, 0, r0, c15, c0, 1	@ read diagnostic register
	orr	r0, r0, #1 << 6		@ set bit #6
	mcr	p15, 0, r0, c15, c0, 1	@ write diagnostic register
#endif

#ifdef CONFIG_ARM_ERRATA_751472
	mrc	p15, 0, r0, c15, c0, 1	@ read diagnostic register
	orr	r0, r0, #1 << 11	@ set bit #11
	mcr	p15, 0, r0, c15, c0, 1	@ write diagnostic register
#endif
#ifdef CONFIG_ARM_ERRATA_761320
	mrc	p15, 0, r0, c15, c0, 1	@ read diagnostic register
	orr	r0, r0, #1 << 21	@ set bit #21
	mcr	p15, 0, r0, c15, c0, 1	@ write diagnostic register
#endif

	mov	pc, lr			@ back to my caller
ENDPROC(cpu_init_cp15)

#ifndef CONFIG_SKIP_LOWLEVEL_INIT
/*************************************************************************
 *
 * CPU_init_critical registers
 *
 * setup important registers
 * setup memory timing
 *
 *************************************************************************/
ENTRY(cpu_init_crit)
	/*
	 * Jump to board specific initialization...
	 * The Mask ROM will have already initialized
	 * basic memory. Go here to bump up clock rate and handle
	 * wake up conditions.
	 */
	b	lowlevel_init		@ go setup pll,mux,memory
ENDPROC(cpu_init_crit)
#endif

#if defined(CONFIG_SUNXI_MULITCORE_BOOT)
/*************************************************************************
 *
 * secondary cpu init
 *
 * setup important registers
 *
 *
 *************************************************************************/

	.globl  get_core_pos
	.globl  secondary_cpu_start
	.globl  third_cpu_start

ENTRY(secondary_cpu_start)

	mrs	r0, cpsr
	bic	r0, r0, #0x1f		@ clear all mode bits
	orr	r0, r0, #0x13		@ set SVC mode
	orr	r0, r0, #(0x40 | 0x80)@ disable FIQ & IRQ
	bic r0, r0, #(1<<9)     @ set little-endian
	msr	cpsr,r0

	mrc	p15, 0, r0, c1, c0, 0
#ifdef CONFIG_ARCH_SUN8IW10P1
	orr	r0, r0, #0x00002000	@ set bits 13 (--V-)
#else
	bic	r0, r0, #0x00002000	@ clear bits 13 (--V-)
#endif
	bic	r0, r0, #0x00000007	@ clear bits 2:0 (-CAM)
	orr	r0, r0, #0x00000800	@ set bit 11 (Z---) BTB
	orr	r0, r0, #0x00001000	@ clear bit 12 (I) I-cache
	mcr	p15, 0, r0, c1, c0, 0

	/* Set vector address in CP15 VBAR register */
	ldr     r0, =_start
	mcr     p15, 0, r0, c12, c0, 0  @Set VBAR

	/*
	 *clear AFE,TRE bit in sctrl
	 *non-secure: reset value is unknow
	 *secure    : default value is 0
	 *notice    : we must set the TRE bit to enable the memory
	 *            arttribute configuration from the section table.
	 */
	MRC     p15, 0, r0, c1, c0, 0   @Read SCTLR
#ifdef CONFIG_ARM_A53
	BIC     r0, r0, #(1<<28)        @clr TRE bit
	BIC     r0, r0, #(1<<29)        @clr AEF bit
#endif
	bic	    r0, #CR_V		        @ V = 0
	MCR     p15, 0, r0, c1, c0, 0   @Write SCTLR

	@ set sp
	ldr r0, =secondary_cpu_mode_stack
	ldr sp, [r0, #0x0]

	@ set r9
	ldr r0, =secondary_cpu_data_groups
	ldr r9, [r0, #0x4]

	@ set irq sp
	mrs r0, cpsr
	bic	r0, r0, #0x1f		@ clear all mode bits
	orr	r0, r0, #0x12		@ set IRQ mode
	msr	cpsr ,r0
	ldr r1, =secondary_cpu_mode_stack
	ldr sp, [r1, #0x4]
	bic	r0, r0, #0x1f		@ clear all mode bits
	orr	r0, r0, #0x13		@ set SVC mode
	msr	cpsr, r0

	b  sunxi_secendary_cpu_task
ENDPROC(secondary_cpu_start)


/*************************************************************************
 *
 * secondary cpu init
 *
 * setup important registers
 *
 *
 *************************************************************************/

ENTRY(third_cpu_start)

	mrs	r0, cpsr
	bic	r0, r0, #0x1f		@ clear all mode bits
	orr	r0, r0, #0x13		@ set SVC mode
	orr	r0, r0, #(0x40 | 0x80)@ disable FIQ & IRQ
	bic r0, r0, #(1<<9)     @ set little-endian
	msr	cpsr,r0

	mrc	p15, 0, r0, c1, c0, 0
#ifdef CONFIG_ARCH_SUN8IW10P1
	orr	r0, r0, #0x00002000	@ set bits 13 (--V-)
#else
	bic	r0, r0, #0x00002000	@ clear bits 13 (--V-)
#endif
	bic	r0, r0, #0x00000007	@ clear bits 2:0 (-CAM)
	orr	r0, r0, #0x00000800	@ set bit 11 (Z---) BTB
	orr	r0, r0, #0x00001000	@ clear bit 12 (I) I-cache
	mcr	p15, 0, r0, c1, c0, 0

	/* Set vector address in CP15 VBAR register */
	ldr     r0, =_start
	mcr     p15, 0, r0, c12, c0, 0  @Set VBAR

	/*
	 *clear AFE,TRE bit in sctrl
	 *non-secure: reset value is unknow
	 *secure    : default value is 0
	 *notice    : we must set the TRE bit to enable the memory
	 *            arttribute configuration from the section table.
	 */
	MRC     p15, 0, r0, c1, c0, 0   @Read SCTLR
#ifdef CONFIG_ARM_A53
	BIC     r0, r0, #(1<<28)        @clr TRE bit
	BIC     r0, r0, #(1<<29)        @clr AEF bit
#endif
	bic	    r0, #CR_V		        @ V = 0
	MCR     p15, 0, r0, c1, c0, 0   @Write SCTLR

#ifndef CONFIG_ARM_A53
	mrc	p15, 0, r0, c1, c0, 0	@ Read CP15 SCTRL Register
	bic	r0, #CR_V		@ V = 0
	mcr	p15, 0, r0, c1, c0, 0	@ Write CP15 SCTRL Register
#endif

	@ set sp
	ldr r0, =secondary_cpu_mode_stack
	ldr sp, [r0, #0x8]

	@ set r9
	ldr r0, =secondary_cpu_data_groups
	ldr r9, [r0, #0x4]

	b  sunxi_third_cpu_task
ENDPROC(third_cpu_start)

/* void cpu_spin_lock(lock address) - lock mutex */
ENTRY(cpu_spin_lock)
    mov r2, #SPINLOCK_LOCK
_spinlock_loop:
    ldrex r1, [r0]
    cmp r1, #SPINLOCK_UNLOCK
    wfene
    bne _spinlock_loop
    strex r1, r2, [r0]
    cmp r1, #0
    wfene
    bne _spinlock_loop
    dmb
    bx lr
ENDPROC(cpu_spin_lock)

/* int cpu_spin_trylock(lock address) - return 0 on success */
ENTRY(cpu_spin_trylock)
    mov r2, #SPINLOCK_LOCK
    mov r1, r0
_trylock_loop:
    ldrex r0, [r1]
    cmp r0, #0
    bne _trylock_out
    strex r0, r2, [r1]
    cmp r0, #0
    bne _trylock_loop
    dmb
    bx lr
_trylock_out:
    clrex
    dmb
    bx lr
ENDPROC(cpu_spin_trylock)

/* void cpu_spin_unlock(lock address) - unlock mutex */
ENTRY(cpu_spin_unlock)
    dmb
    mov r1, #SPINLOCK_UNLOCK
    str r1, [r0]
    dsb
    sev
    bx lr
ENDPROC(cpu_spin_unlock)
#endif

ENTRY(get_core_pos)

	mrc	p15, 0, r0, c0, c0, 5
	and	r1, r0, #0xff
	and	r0, r0, #0xff00
	/*
	 * Number of cores in cluster is 2,
	 * we should have the following mapping:
	 * MPIDR     core_pos
	 * 0x0000 -> 0
	 * 0x0001 -> 1
	 * 0x0100 -> 2
	 * 0x0101 -> 3
	 */
	add	r0, r1, r0, LSR #7
	bx	lr
ENDPROC(get_core_pos)

.globl  cpu0_set_irq_stack;

ENTRY(cpu0_set_irq_stack)


	@ set irq sp
	mrs r1, cpsr
	bic	r1, r1, #0x1f		@ clear all mode bits
	orr	r1, r1, #0x12		@ set IRQ mode
	msr	cpsr, r1
	mov sp, r0
	bic	r1, r1, #0x1f		@ clear all mode bits
	orr	r1, r1, #0x13		@ set SVC mode
	msr	cpsr, r1

	bx  lr
ENDPROC(cpu0_set_irq_stack)
